
#include "bsp_lidar_sensor.h"

#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "FreeRTOS.h"
#include "ax_oled_1305.h"
#include "bsp_def.h"
#include "cmsis_os.h"
#include "lidar_ld06.h"
#include "main.h"
#include "obstacle_avoidance.h"
#include "task.h"
#include "usart.h"

static uint8_t lidar_rx_buffer[64] = {0};  // 存储激光雷达信息47字节 波特率230400 8 1
LiDARFrameTypeDef ld06_frame;
// 初始化避障系统
ObstacleAvoidanceSystem obstacle_system;

void BSP_Lidar_Init() {
 
    HAL_UARTEx_ReceiveToIdle_DMA(&huart2, lidar_rx_buffer, sizeof(lidar_rx_buffer));  // 启动DMA接收,用来接收激光雷达数据
    __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT);                                   // 关闭DMA接收缓冲区半满中断

    printf("=== lidar start ===\n");

    InitObstacleSystem(&obstacle_system, 200, 500);  // 紧急停止200mm, 安全距离500mm

    printf("sys init complete - stop: %dmm, safe: %dmm\n",
           obstacle_system.emergency_stop_distance,
           obstacle_system.safe_distance);
}

void BSP_Lidar_Loop(void) {
    while (1) {
        // LiDARFrameTypeDef frame;
        // uint8_t radar_data[47]; // 47字节数据包
        // 处理避障决策
        ObstacleAction action = ProcessObstacleAvoidance(&obstacle_system, &ld06_frame);

        // 获取距离信息
        uint16_t front_dist, left_dist, right_dist;
        GetMultiDirectionDistances(&obstacle_system, &front_dist, &left_dist, &right_dist);
        //AX_OLED_DispValue(1, 6, ld06_frame.round, 8, 0, 1);
        // 显示信息
        //    printf("info:[%4lu] F: %3dmm, L: %3dmm, R: %3dmm -> ",
        //           frame_count, front_dist, left_dist, right_dist);
        // 延时模拟实时控制（根据实际雷达数据率调整）

        osDelay(20);  // 约417Hz，对应2.4ms
    }
}
void BSP_LEADAR_UARTEx_RxEventCallback(UART_HandleTypeDef* huart, uint16_t Size) {
    if (huart->Instance == BSP_LIDAR_DETECT_INSTANCE)  // UART2接收中断处理函数
    {
        uint8_t rxbuff[64] = {0};
        memcpy(rxbuff, lidar_rx_buffer, Size);
        //HAL_UART_Transmit_IT(&huart1, rxbuff, strlen((char*)rxbuff));
        if (ParseLidarFrame(&ld06_frame, rxbuff)) {  // 解析并保存函数

            //AX_OLED_DispValue(60, 6, ld06_frame.last_round_min_distance, 5, 0, 0);
            // AX_OLED_DispValue(0, 7, ld06_frame.distance, 5, 0, 0);
            //    printf("destance: %d \n", ld06_frame.round_min_distance);
            // uint8_t tmp[64] = {0};
            // sprintf((char*)tmp, "angle:%d,d:%ld,min:%d\n", ld06_frame.start_angle, ld06_frame.distance, ld06_frame.round_min_distance);
            //AX_OLED_DispValue(0, 7, ld06_frame.round_min_distance, 8, 0, 1);
            // 计算实际字符串长度，避免发送多余的空字节
            // size_t len = strlen((char*)tmp);
            // HAL_UART_Transmit_IT(&huart1, tmp, len);
        }

        HAL_UARTEx_ReceiveToIdle_DMA(&huart2, lidar_rx_buffer, sizeof(lidar_rx_buffer));  // 启动DMA接收
        __HAL_DMA_DISABLE_IT(huart2.hdmarx, DMA_IT_HT);                                   // 关闭DMA接收缓冲区溢出中断
    }
}